/*
 * Copyright (C) 2015-2018 Alibaba Group Holding Limited
 */

#include <mbmaster_api.h>
#include "pdu.h"

#define PDU_REQ_READ_ADDR_OFF   (PDU_DATA_FIELD_OFF + 0)
#define PDU_REQ_READ_REGCNT_OFF (PDU_DATA_FIELD_OFF + 2)
#define PDU_REQ_WRITE_LENGTH_OFF (PDU_DATA_FIELD_OFF + 4)
#define PDU_REQ_WRITE_DATA_OFF (PDU_DATA_FIELD_OFF + 5)
#define PDU_REQ_READ_SIZE       (5)
#define PDU_REQ_WRITE_SIZE       (6)
#define PDU_REQ_COIL_SIZE       (5)
#define PDU_REQ_READ_REGCNT_MAX (0x007D)

#define PDU_RESP_READ_REGCNT_OFF (PDU_DATA_FIELD_OFF + 0)
#define PDU_RESP_READ_VALUE_OFF  (PDU_DATA_FIELD_OFF + 1)

#if (MBMASTER_CONFIG_READ_HOLDING > 0)
mb_status_t read_holding_reginster_assemble(mb_handler_t *req_handler, uint16_t start_addr, uint16_t quantity)
{
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
        status = MB_INVALID_SLAVE_ADDR;
        return status;
    }

    pdu_buf[PDU_FUNC_FIELD_OFF]        = FUNC_CODE_READ_HOLDING_REGISTERS;
    pdu_buf[PDU_REQ_READ_ADDR_OFF]     = htobe(start_addr);
    pdu_buf[PDU_REQ_READ_ADDR_OFF+1]   = htobe(start_addr) >> 8;
    pdu_buf[PDU_REQ_READ_REGCNT_OFF]   = htobe(quantity);
    pdu_buf[PDU_REQ_READ_REGCNT_OFF+1] = htobe(quantity) >> 8;

    req_handler->pdu_length = PDU_REQ_READ_SIZE;

    return status;
}

mb_status_t write_holding_reginster_assemble(mb_handler_t *req_handler, uint16_t start_addr, uint16_t quantity, int16_t *buf)
{
    mb_status_t status  = MB_SUCCESS;
	int i = 0, j = 0;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
        status = MB_INVALID_SLAVE_ADDR;
        return status;
    }

	if (quantity > WRITE_BATE_MAX) {
        status = MB_INVALID_PARAM;
        return status;
    }

	if (NULL == buf) {
        status = MB_INVALID_PARAM;
        return status;
    }

    pdu_buf[PDU_FUNC_FIELD_OFF]        = FUNC_CODE_WRITE_MULTIPLE_REG;
    pdu_buf[PDU_REQ_READ_ADDR_OFF]     = htobe(start_addr);
    pdu_buf[PDU_REQ_READ_ADDR_OFF+1]   = htobe(start_addr) >> 8;
    pdu_buf[PDU_REQ_READ_REGCNT_OFF]   = htobe(quantity);
    pdu_buf[PDU_REQ_READ_REGCNT_OFF+1] = htobe(quantity) >> 8;
	pdu_buf[PDU_REQ_WRITE_LENGTH_OFF]  = quantity << 1;

	for(i = 0 , j = 0; j < quantity; i+=2, j++)
	{
		pdu_buf[PDU_REQ_WRITE_DATA_OFF + i] = htobe(buf[j]);
		pdu_buf[PDU_REQ_WRITE_DATA_OFF + i + 1] = htobe(buf[j]) >> 8;
	}
	
    req_handler->pdu_length = PDU_REQ_WRITE_SIZE + pdu_buf[PDU_REQ_WRITE_LENGTH_OFF];

    return status;
}

mb_status_t write_coil_reginster_assemble(mb_handler_t *req_handler, uint16_t addr, uint16_t val)
{
    mb_status_t status  = MB_SUCCESS;
	// int i = 0, j = 0;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
        status = MB_INVALID_SLAVE_ADDR;
        return status;
    }

    pdu_buf[PDU_FUNC_FIELD_OFF]        = FUNC_CODE_WRITE_COIL_REG;
    pdu_buf[PDU_REQ_READ_ADDR_OFF]     = htobe(addr);
    pdu_buf[PDU_REQ_READ_ADDR_OFF+1]   = htobe(addr) >> 8;
    pdu_buf[PDU_REQ_READ_REGCNT_OFF]   = htobe(val);
    pdu_buf[PDU_REQ_READ_REGCNT_OFF+1] = htobe(val) >> 8;

    req_handler->pdu_length = PDU_REQ_COIL_SIZE;

    return status;
}

mb_status_t read_coil_reginster_assemble(mb_handler_t *req_handler, uint16_t start_addr, uint16_t quantity)
{
	mb_status_t status	= MB_SUCCESS;
	uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

	if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
		status = MB_INVALID_SLAVE_ADDR;
		return status;
	}

	pdu_buf[PDU_FUNC_FIELD_OFF] 	   = FUNC_CODE_READ_COIL_REG;
	pdu_buf[PDU_REQ_READ_ADDR_OFF]	   = htobe(start_addr);
	pdu_buf[PDU_REQ_READ_ADDR_OFF+1]   = htobe(start_addr) >> 8;
	pdu_buf[PDU_REQ_READ_REGCNT_OFF]   = htobe(quantity);
	pdu_buf[PDU_REQ_READ_REGCNT_OFF+1] = htobe(quantity) >> 8;

	req_handler->pdu_length = PDU_REQ_READ_SIZE;

	return status;
}


mb_status_t read_holding_reginster_disassemble(mb_handler_t *req_handler, uint8_t *respond_buf,
                                               uint8_t *respond_count)
{
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if ((respond_buf == NULL) || (respond_count == NULL)) {
        status = MB_INVALID_PARAM;
        return status;
    }

    if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
        respond_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
        *respond_count  = 1;

        status = MB_RESPOND_EXCEPTION;
        return status;
    }

    *respond_count = pdu_buf[PDU_RESP_READ_REGCNT_OFF];

    if ((*respond_count + PDU_RESP_READ_VALUE_OFF) != req_handler->pdu_length) {
        status = MB_RESPOND_LENGTH_ERR;
        return status;
    }

    memcpy(respond_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *respond_count);
    return status;
}


mb_status_t write_holding_reginster_disassemble(mb_handler_t *req_handler, uint8_t *respond_buf,
                                               uint8_t *respond_count)
{
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if ((respond_buf == NULL) || (respond_count == NULL)) {
        status = MB_INVALID_PARAM;
        return status;
    }

    if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
        respond_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
        *respond_count  = 1;

        status = MB_RESPOND_EXCEPTION;
        return status;
    }

    *respond_count = 2;

    memcpy(respond_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *respond_count);
    return status;
}   

											   
mb_status_t write_coil_reginster_disassemble(mb_handler_t *req_handler, uint8_t *respond_buf,
											  uint8_t *respond_count)
{
   mb_status_t status  = MB_SUCCESS;
   uint8_t	  *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

   if ((respond_buf == NULL) || (respond_count == NULL)) {
	   status = MB_INVALID_PARAM;
	   return status;
   }

   

   if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
	   respond_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
	   *respond_count  = 1;

	   status = MB_RESPOND_EXCEPTION;
	   return status;
   }

   *respond_count  = 2;
   memcpy(respond_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *respond_count);
   return status;
}   

mb_status_t read_coil_reginster_disassemble(mb_handler_t *req_handler, uint8_t *respond_buf,
											uint8_t *respond_count)
{
  mb_status_t status  = MB_SUCCESS;
  uint8_t	 *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

  if ((respond_buf == NULL) || (respond_count == NULL)) {
	  status = MB_INVALID_PARAM;
	  return status;
  }

  if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
	  respond_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
	  *respond_count  = 1;

	  status = MB_RESPOND_EXCEPTION;
	  return status;
  }

  *respond_count = pdu_buf[PDU_RESP_READ_REGCNT_OFF];

  if ((*respond_count + PDU_RESP_READ_VALUE_OFF) != req_handler->pdu_length) {
	  status = MB_RESPOND_LENGTH_ERR;
	  return status;
  }

  memcpy(respond_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *respond_count);
  return status;
}

/*
be in common use write reginster assemble
designed by lunanting
param:
    req_handler:modbus handler
    fun_code:function code
    start_addr:write start address
    w_buf:write buffer
    w_len:write length
return:mb_status_t
*/
mb_status_t write_single_reginster_assemble(mb_handler_t *req_handler,uint16_t start_addr,uint16_t *w_buf,uint16_t w_len){
    mb_status_t status  = MB_SUCCESS;
	int i = 0, j = 0;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
        status = MB_INVALID_SLAVE_ADDR;
        return status;
    }

	if (w_len > WRITE_BATE_MAX) {
        status = MB_INVALID_PARAM;
        return status;
    }

	if (NULL == w_buf) {
        status = MB_INVALID_PARAM;
        return status;
    }

    pdu_buf[0]     = FUNC_CODE_WRITE_SINGLE_REG;
    pdu_buf[1]     = htobe(start_addr);
    pdu_buf[1+1]   = htobe(start_addr) >> 8;

	for(i = 0 , j = 0; j < 1; i+=2, j++)
	{
		pdu_buf[3 + i] = htobe(w_buf[j]);
		pdu_buf[3 + i + 1] = htobe(w_buf[j]) >> 8;
	}
	
    req_handler->pdu_length = 3 + 2;

    return status;
}

/*
be in common use read regsinter
designed by lunanting
param:
    req_handler:modbus handler
    fun_code:modbus function code
    start_addr:start address
    read_len:read length
return:mb_status_t
*/
mb_status_t read_single_reginster_assemble(mb_handler_t *req_handler,uint16_t start_addr,uint16_t read_len){
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if (req_handler->slave_addr > SLAVE_ADDR_MAX) {
        status = MB_INVALID_SLAVE_ADDR;
        return status;
    }

    pdu_buf[PDU_FUNC_FIELD_OFF]        = FUNC_CODE_READ_HOLDING_REGISTERS;
    pdu_buf[PDU_REQ_READ_ADDR_OFF]     = htobe(start_addr);
    pdu_buf[PDU_REQ_READ_ADDR_OFF+1]   = htobe(start_addr) >> 8;
    pdu_buf[PDU_REQ_READ_REGCNT_OFF]   = htobe(read_len);
    pdu_buf[PDU_REQ_READ_REGCNT_OFF+1] = htobe(read_len) >> 8;

    req_handler->pdu_length = PDU_REQ_READ_SIZE;

    return status;
}

/*
be in common use write disassemble
designed by lunanting
param:
    req_handler；modbus handler
    res_buf:response data saving buffer
    res_len:resonse data length saving pointer
return:mb_status_t
*/
mb_status_t write_single_reginster_disassemble(mb_handler_t *req_handler,uint8_t *res_buf,uint8_t *res_len){
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if ((res_buf == NULL) || (res_len == NULL)) {
        status = MB_INVALID_PARAM;
        return status;
    }

    if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
        res_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
        *res_len  = 1;

        status = MB_RESPOND_EXCEPTION;
        return status;
    }

    *res_len = 2;

    memcpy(res_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *res_len);
    return status;
}


/*
be in common use read reginster disassemble
designed by lunanting
param:
    req_handler；modbus handler
    res_buf:response data saving buffer
    res_len:resonse data length saving pointer
return:mb_status_t
*/
mb_status_t read_single_reginster_disassemble(mb_handler_t *req_handler,uint8_t *res_buf,uint8_t *res_len){
    mb_status_t status  = MB_SUCCESS;
    uint8_t    *pdu_buf = &req_handler->mb_frame_buff[req_handler->pdu_offset];

    if ((res_buf == NULL) || (res_len == NULL)) {
        status = MB_INVALID_PARAM;
        return status;
    }

    if (pdu_buf[PDU_FUNC_FIELD_OFF] & 0x80) {
        res_buf[0] = pdu_buf[PDU_RESP_READ_REGCNT_OFF];
        *res_len  = 1;

        status = MB_RESPOND_EXCEPTION;
        return status;
    }

    *res_len = pdu_buf[PDU_RESP_READ_REGCNT_OFF];

    if ((*res_len + PDU_RESP_READ_VALUE_OFF) != req_handler->pdu_length) {
        status = MB_RESPOND_LENGTH_ERR;
        return status;
    }

    memcpy(res_buf, &pdu_buf[PDU_RESP_READ_VALUE_OFF], *res_len);
    return status;
}

#endif /* MBMASTER_CONFIG_READ_HOLDING */
